package simulator.robot;

import lejos.robotics.RegulatedMotor;
import lejos.robotics.RegulatedMotorListener;
import Program.Program;

public class SensorMotor implements RegulatedMotor, Runnable {
	
	private static final int UPDATES_PER_SECOND = 100;
	private static final float MAX_ROTATE_SPEED = 301.0F;
	private double rotation = 0.0;
	private int rotateTo = 0;
	private int speed = (int) (getMaxSpeed() * 0.9);
	private SimulatorListener listener;
	
	private boolean isMoving = false;
	
	public SensorMotor() {
		new Thread(this);
	}

	@Override
	public synchronized void run() {
		while (true) {
			try {
				this.wait();
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
			rotate();
		}
	}

	@Override
	public void forward() {
		throw new UnsupportedOperationException();
	}

	@Override
	public void backward() {
		throw new UnsupportedOperationException();
	}

	@Override
	public void stop() {
		throw new UnsupportedOperationException();
	}

	@Override
	public void flt() {
		throw new UnsupportedOperationException();
	}

	@Override
	public boolean isMoving() {
		return isMoving;
	}

	@Override
	public int getRotationSpeed() {
		return 15;
	}

	@Override
	public int getTachoCount() {
		return (int) rotation;
	}

	@Override
	public void resetTachoCount() {
		rotation = 0;
	}

	@Override
	public void addListener(RegulatedMotorListener listener) {
		throw new UnsupportedOperationException();
		
	}

	@Override
	public RegulatedMotorListener removeListener() {
		throw new UnsupportedOperationException();
	}

	@Override
	public void stop(boolean immediateReturn) {
		throw new UnsupportedOperationException();
		
	}

	@Override
	public void flt(boolean immediateReturn) {
		throw new UnsupportedOperationException();
		
	}

	@Override
	public void waitComplete() {
		throw new UnsupportedOperationException();
		
	}

	@Override
	public void rotate(int angle, boolean immediateReturn) {
		throw new UnsupportedOperationException();
	}

	@Override
	public void rotate(int angle) {
		throw new UnsupportedOperationException();
	}
	
	private void rotate() {
		isMoving = true;
		
		double rotateBound = rotateTo;
//		System.out.println(rotateTo+"/"+rotation);
		while (Double.compare(rotateBound, rotateTo)==0 && Double.compare(rotation,rotateTo)!=0) {
			rotation += Math.signum(rotateTo-rotation)*getRotationSpeed();
			//System.out.println(rotation);
			Program.simulatorGui.setSensorAngle(rotation);
			try {
				Thread.sleep(1000 / UPDATES_PER_SECOND);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		
		isMoving = false;
//		listener.updateSensorAngle();
//		listener.updateSonarDistance();
	}

	@Override
	public void rotateTo(int limitAngle) {
		rotateTo(limitAngle, false);
	}

	@Override
	public synchronized void rotateTo(int limitAngle, boolean immediateReturn) {
		rotateTo = limitAngle;

		if (!immediateReturn) {
			rotate();
		}
		else {
			this.notify();
		}
	}

	@Override
	public int getLimitAngle() {
		return rotateTo;
	}

	@Override
	public void setSpeed(int speed) {
		this.speed = speed;
	}

	@Override
	public int getSpeed() {
		return speed;
	}

	@Override
	public float getMaxSpeed() {
		return MAX_ROTATE_SPEED;
	}

	@Override
	public boolean isStalled() {
		throw new UnsupportedOperationException();
	}

	@Override
	public void setStallThreshold(int error, int time) {
		throw new UnsupportedOperationException();
		
	}

	@Override
	public void setAcceleration(int acceleration) {
		throw new UnsupportedOperationException();
	}
	
	public void setListener(SimulatorListener listener) {
	    this.listener = listener;
	}
}
